// 通过蓝牙透传模块连接手机APP

/*
 * 作者：勤为本
 * 勤为本的小店：https://shop333183734.taobao.com/
 */

#include <math.h>
#include <stdlib.h>
#include "../lib/ls1c_public.h"
#include "../lib/ls1c_uart.h"
#include "../lib/start.h"
#include "../lib/ls1c_irq.h"
#include "../lib/ls1c_pin.h"
#include "../lib/ls1c_delay.h"
#include "motor.h"


// 蓝牙透传模块所在引脚
#define BLUETOOTH_UART_TX 		(50)	// CAMDATA0/GPIO50/UART10_TX
#define BLUETOOTH_UART_RX 		(51)	// CAMDATA1/GPIO51/UART10_RX


#define BLUETOOTH_CMD_PID_INIT 		(0)
#define BLUETOOTH_CMD_PID_START		(1)
#define BLUETOOTH_CMD_PID_END		(2)


// 蓝牙透传模块所在串口
ls1c_uart_info_t bluetooth_uart_info = {0};

// 串口寄存器基地址
void *bluetooth_uart_base = NULL;

unsigned char bluetooth_send_pid_info_flag = 0;
unsigned char bluetooth_save_pid_info_flag = 0;


extern float angle_init;
extern float angle_balance;
extern float gyro_balance;
extern float gyro_turn;
extern float accel_z;

extern int balance_kp;
extern int balance_kd;


// 电机的速度
extern volatile int motor_left_speed;
extern volatile int motor_right_speed;


/*
 * 蓝牙透传模块所在串口的中断处理函数
 * @IRQn 中断号
 * @param 传递给中断处理函数的参数
 */
void bluetooth_uart_irqhandler(int IRQn, void *param)
{
	unsigned char iir = reg_read_8(bluetooth_uart_base + LS1C_UART_IIR_OFFSET);
	unsigned char data = 0;
	static unsigned char buf[64] = {0};
	static unsigned char flag_pid = 0;
	static int i = 0;
	int j = 0;
	int cmd_value = 0;
	int pow_value = 0;

	// 判断是否为接收超时，或接收到有效数据
	if ((IIR_RXTOUT & iir) || (IIR_RXRDY & iir))
	{
		// 是，则读取数据
		while (LSR_RXRDY & reg_read_8(bluetooth_uart_base + LS1C_UART_LSR_OFFSET))
		{
			// 读出一字节数据
			data = reg_read_8(bluetooth_uart_base + LS1C_UART_DAT_OFFSET);

			if ('{' == data) flag_pid = BLUETOOTH_CMD_PID_START;	// 指令起始位'{'
			if ('}' == data) flag_pid = BLUETOOTH_CMD_PID_END;		// 指令结束位'}'
			
			// 记录命令内容
			if (BLUETOOTH_CMD_PID_START == flag_pid)
			{
				buf[i++] = data;
			}

			// 执行命令
			if (BLUETOOTH_CMD_PID_END == flag_pid)
			{
				buf[i] = '}';		// 补上指令结束符'}'
				buf[i+1] = '\0';	// 字符串结束符，方便打印字符串
//				printf("[%s] cmd=%s\r\n", __FUNCTION__, buf);	// 打印命令到调试串口
				
				if (0x50 == buf[3])	bluetooth_send_pid_info_flag = 1;		// 获取设备参数
				else if (0x57 == buf[3]) bluetooth_save_pid_info_flag = 1;	// 掉电保存参数
				else if (0x23 != buf[1])	// 更新pid参数
				{
					// 从命令中取出pid参数
					pow_value = 1;
					for (j = i; j >= 4; j--)
					{
						cmd_value += (buf[j - 1] - '0') * pow_value;
						pow_value *= 10;
					}
//					printf("[%s] cmd_value=%d\r\n", __FUNCTION__, cmd_value);

					switch (buf[1])
					{
						case '0':			// 手机app修改的第一个参数（初始倾角）
							angle_init = 10 - cmd_value;
							printf("[%s] new angle_init=%f\r\n", __FUNCTION__, angle_init);
							break;
							
						case '1':			// 手机app修改的第二个参数（平衡直立的p系数）
							balance_kp = cmd_value;
							printf("[%s] new balance_kp=%d\r\n", __FUNCTION__, balance_kp);
							break;

						case '2':			// 手机app修改的第三个参数（平衡直立的d系数）
							balance_kd = cmd_value;
							printf("[%s] new balance_kd=%d\r\n", __FUNCTION__, balance_kd);
							break;
						default:
							break;
					}
				}

				// 清零相关标志位
				i = 0;
				flag_pid = BLUETOOTH_CMD_PID_INIT;
			}
		}
	}

	return ;
}

 
// 蓝牙透传模块初始化
void bluetooth_init(void)
{
	// 设置引脚复用
	pin_set_remap(BLUETOOTH_UART_TX, PIN_REMAP_FIFTH);
	pin_set_remap(BLUETOOTH_UART_RX, PIN_REMAP_FIFTH);

	// 初始化蓝牙透传模块所在的串口
	bluetooth_uart_info.UARTx 		= LS1C_UART10;
	bluetooth_uart_info.baudrate 	= 9600;
	bluetooth_uart_info.rx_enable 	= TRUE;
	uart_init(&bluetooth_uart_info);

	// 设置中断处理函数
	irq_install(LS1C_UART10_IRQ, bluetooth_uart_irqhandler, NULL);
	irq_enable(LS1C_UART10_IRQ);

	bluetooth_uart_base = uart_get_base(bluetooth_uart_info.UARTx);

	return ;
}


/*
 * 通过蓝牙透传指定的字符串
 * @str 待发送的字符串
 */
inline void bluetooth_print(const char *str)
{
	uart_print(bluetooth_uart_info.UARTx, str);
	return ;
}


/*
 * 发送状态(速度和倾角)
 */
void bluetooth_send_status(void)
{
	int angle = 0;				// 倾角
	int test = 0;
	char buf[128] = {0};

	angle = (int)angle_balance;
	sprintf(buf, "{A%u:%u:%d:%d}$", 
			abs(motor_left_speed)*100/MOTOR_SPEED_MAX, 		// 将速度转换为百分比
			abs(motor_right_speed)*100/MOTOR_SPEED_MAX, 
			test, 
			angle);
	bluetooth_print(buf);

	return ;
}


// 发送曲线
void bluetooth_send_curve(void)
{
	int angle_real = 0;			// 减去装配误差后的实际倾角
	char buf[128] = {0};

	angle_real = (int)angle_balance - (int)angle_init;
	sprintf(buf, 
			"{B%d:%d:%d}$", 
			angle_real, 
			motor_left_speed,
			motor_right_speed);
	bluetooth_print(buf);

	return ;
}


